﻿using UnityEngine;
using System.Collections;

public class KinematicArrive : MonoBehaviour {
	//the invisible leader we will be following
	public Transform invisibleLeader;
	//A reference to the space coordinates of our current target desination
	public Vector3 target = new Vector3();
	//save a reference to our final target after a collision occurs
	public Vector3 finalTarget;
	//The maximum speed our object may travel
	public float maxSpeed = .1f;
	//The radius around our target destination that is considered close enough that we have arrived.
	public float radius;
	//the time to target constant
	public float timeToTarget = .1f;
	//the last position the mouse clicked at
	public Vector3 myMousePosition = new Vector3();

	public bool isMoving = false;
	//lerp variables
	public Quaternion from;

	//collider booleans
	public bool collidedWithObject;
	public bool colliding;
	public int numCollisions;
	public Vector3 obstaclePosition;
	// Use this for initialization
	void Start () {
		//initialize the target position, radius of satisfaction, and the maximum speed multiplier
		target = transform.position;
		radius = .4f;
		maxSpeed = 10f;
		collidedWithObject = false;
		numCollisions = 0;
	}
	
	// Update is called once per frame
	void Update () {
		//If the user clicks on the screen and they are not currently moving, get their mouse position, and set isMoving to true
		/*if (Input.GetMouseButtonDown(0) && !isMoving){
			myMousePosition = Input.mousePosition;
			myMousePosition.z = 10f;
			target = Camera.main.ScreenToWorldPoint(new Vector3(myMousePosition.x, myMousePosition.y, myMousePosition.z));
			finalTarget = target;
			isMoving = true;
			from = transform.rotation;
		}*/
		if (Input.GetMouseButtonDown (0)) {
			//finalTarget = Camera.main.ScreenToWorldPoint(new Vector3(myMousePosition.x, myMousePosition.y, myMousePosition.z));;
		}
		if ((transform.position - invisibleLeader.position).magnitude > radius) {
			isMoving = true;
			from = transform.rotation;
			if(!colliding){
				target = invisibleLeader.position;
			}
			finalTarget = target;
		}
		//if you collide with an object, go around it
		if(collidedWithObject){
			if((transform.position + (transform.right*10) - obstaclePosition).magnitude < (transform.position + (transform.right*-10)).magnitude){
				target = transform.position + (transform.right * 10);
			}
			else{
				target = transform.position + (transform.right * -10);
			}
			collidedWithObject = false;
			colliding = true;
		}
		if (!colliding)
			target = finalTarget;

		//if the objects isMoving var is true, then perform the movement logic.
		if (isMoving) {
			KinematicSteeringOutput Steering = steering ();
			transform.position += transform.forward * Steering.velocity.magnitude/2 * Time.deltaTime;
		}
	}

	public KinematicSteeringOutput steering(){
		//create the structure for the output
		KinematicSteeringOutput steering = new KinematicSteeringOutput();

		// get the direction to the target
		steering.velocity = target - transform.position;
		// if we are within the radius, we can return false
		if (steering.velocity.magnitude < radius){
			KinematicSteeringOutput emptySteering = new KinematicSteeringOutput();
			isMoving = false;
			return emptySteering;
		}

		//otherwise, we need to move to our target
		//get there in timeToTarget seconds
		steering.velocity /= timeToTarget;

		//see if this is greater than our max speed, if it is, set it equal to max speed.
		if (steering.velocity.magnitude > maxSpeed){
			steering.velocity.Normalize();
			steering.velocity *= maxSpeed;
		}

		//turn to face our target direction
		float newOrientation = getNewOrientation(transform.eulerAngles, steering.velocity);
		//get the rotation you need to be at to face the target
		Quaternion targetRotation = Quaternion.LookRotation (target - transform.position, Vector3.up);
		//Lerp to update rotation smoothly
		transform.rotation = Quaternion.Lerp (transform.rotation, targetRotation, .1f);
		//return
		steering.rotation = new Vector3(0,0,0);
		return steering;
	}

	public float getNewOrientation(Vector3 currentOrientation, Vector3 velocity){
		//verify we should be moving
		if (velocity.magnitude > 0){
			//calculate the orientation busing an arc tangent of the vel components
			return Vector3.Angle(transform.forward, velocity);
		}
		return 1;
	}

	void OnTriggerEnter(Collider other){
		if(other.gameObject.CompareTag("obstacle")){
			collidedWithObject = true;
			numCollisions++;
			obstaclePosition = other.transform.position;
		}
	}

	void OnTriggerStay(Collider other){
		if (other.gameObject.CompareTag ("obstacle")) {
			colliding = true;
		}
	}

	void OnTriggerExit(Collider other){
		if(other.gameObject.CompareTag("obstacle")){
			numCollisions--;
			if(numCollisions==0)
				colliding = false;
		}
	}
}
//A class for keeping an object with a velocity and rotation.
public class KinematicSteeringOutput {
	public Vector3 velocity = new Vector3();
	public Vector3 rotation = new Vector3();

	public KinematicSteeringOutput(){
		velocity = new Vector3 (0, 0, 0);
		rotation = new Vector3 (0, 0, 0);
	}
}